/*********************************************************************
*
*   RobotBase.h - Robot base class
*
*   COPYRIGHT:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/
#pragma once

#include <stdio.h>

#include "RobotMath.h"

class RobotBase {
public:
    /**************************************************************************
    * GetRobotPosition()
    *   Outputs current robot position and heading (in degrees).
    **************************************************************************/
    void GetRobotPosition( float * x, float * y, float * yaw )
    {
        *x   = ( float )m_x;
        *y   = ( float )m_y;
        *yaw = ( float )( m_yaw * 180.0 / PI );
    };

    /**************************************************************************
    * GetMotorSpeed()
    *   Outputs current motor setting as an integer between -100 and 100
    *   (inclusive)
    **************************************************************************/
    void GetMotorSpeed( int * left, int * right )
    {
    *left = ( int )( m_leftMotor * 100 );
    *right = ( int )( m_rightMotor * 100 );
    }

    void Update( );
    
    static RobotBase * p_robot;     // pointer to the last created robot instance

    // The following variables are used to simulate quad encoders
    double leftMotorAccumulator;
    double rightMotorAccumulator;

protected:
    RobotBase( );
    ~RobotBase( );
    
    void SetPos( double x, double y );  //in display units
    void SetAngle( double a );          //angle in degrees clockwise from north
    
    /**************************************************************************
    * SetLeftMotor()
    *   Set left motor speed.
    *   Valid speed is [-1.0,1.0]
    **************************************************************************/
    void SetLeftMotor( double speed )
    {
        SetMotor( &m_leftMotor, speed );
    };

    /**************************************************************************
    * SetRightMotor()
    *   Set right motor speed.
    *   Valid speed is [-1.0,1.0]
    **************************************************************************/
    void SetRightMotor( double speed )
    {
        SetMotor( &m_rightMotor, speed );
    };

    virtual void StudentPeriodic( ) = 0;
    void TestPeriodic( unsigned int t_ms );

private:
    double m_leftMotor;
    double m_rightMotor;
    double m_x;
    double m_y;
    double m_yaw;                   // robot heading in radians
                                    // measured clockwise from the North
    double m_width;                 // robot width
    unsigned long m_ticks;          // simulated milliseconds since start
    FILE *m_logFile;                // log file

    void MoveRobot( );
    void SetMotor( double * p_motor, double speed );
};

